#include "yaogan.hpp"
#include <boost/asio.hpp>
//#include <boost/bind.hpp>
#include <boost/array.hpp>
#include <boost/asio/steady_timer.hpp>
#include <memory>
#include <thread>
#include <functional>
#include <queue>
#include <string>
#include <iostream>
// std::string toHex(uint8_t ch)
// {
//     char buf[8] = {0};
//     snprintf(buf, 8, "%x", ch);
//     return buf;
// }

yaogan::yaogan(std::string port, int baud)
    : Serial(port, baud)
{
    open();
    runThreadFunc();
    channel=25;
    yg_value.Left_horiz=0; yg_value.Left_vertic=0;yg_value.Right_horiz=0;yg_value.Right_vertic=0;
}
YG_value *yaogan::get_yaogan_ptr(void)
{
  rec_flag=0;
  return &yg_value;
}
int yaogan::Get_yaogan_flag()
{
  return rec_flag;
}
void yaogan::recvMsg(std::vector<u_int8_t> &msg)
{
     int value1,value2;
    if(((int)msg[9])!=channel)return;
    if(rec_ok(msg)==0)
    {
     // std::cout << "recv: " << std::endl;
     rec_count++;
     rec_flag=1;
      value1=(int)msg[2]-128;
      if(value1>=0)
        {
            if(value1<LIMIT_0)value1=0;
             else value1=value1-LIMIT_0;
        }
        else 
        {
            if(value1>-LIMIT_0)value1=0;
             else value1=value1+LIMIT_0;
        }
      yg_value.Left_horiz=-value1;
    //////////////////////////  
      value1=(int)msg[3]-128;
      if(value1>=0)
        {
            if(value1<LIMIT_0)value1=0;
             else value1=value1-LIMIT_0;
        }
        else 
        {
            if(value1>-LIMIT_0)value1=0;
             else value1=value1+LIMIT_0;
        }
      yg_value.Left_vertic=-value1;
      ///////////////////
       value1=(int)msg[4]-128;
      if(value1>=0)
        {
            if(value1<LIMIT_0)value1=0;
             else value1=value1-LIMIT_0;
        }
        else 
        {
            if(value1>-LIMIT_0)value1=0;
             else value1=value1+LIMIT_0;
        }
      yg_value.Right_horiz=-value1;
       ///////////////////
       value1=(int)msg[5]-128;
      if(value1>=0)
        {
            if(value1<LIMIT_0)value1=0;
             else value1=value1-LIMIT_0;
        }
        else 
        {
            if(value1>-LIMIT_0)value1=0;
             else value1=value1+LIMIT_0;
        }
      yg_value.Right_vertic=-value1;
      yg_value.key1=msg[6]; yg_value.key2=msg[7];
    }

}
int yaogan::rec_ok(std::vector<u_int8_t> &msg)
{
    int flag=0;
    if(msg[0]!=0xaa)return 1;
    if(msg[1]!=0x0a)return 1;
    if(msg[11]!=0xbb)return 1;
    return flag;
    
}
